/** 
 * try to switched between task0 and  task1.
 * switching implemented in isr_wrapper of vectors.s
 * hzheng, 2023.08.24
 */
#include "main.h"

int printf_(const char* format, ...);//implemented in printf.c
extern unsigned int *saveRegToStack(void); //implemented in vectors.s

void mydelay(void);
void yield(void);
void invokeEcall(unsigned int argA0, unsigned int argA1);
void initTCB(void);
void initTaskContext(TaskContext *context);
TaskCtrl* switchCurrentTask(void);

//for .s debug
void showMsg(unsigned int i, unsigned int data);

//task control related definition
//
TaskCtrlBlock gTCB0, gTCB1;
unsigned int gCurrentTaskId;
__attribute__((aligned(4))) unsigned int gTask1Stack[217];
__attribute__((aligned(4))) unsigned int gTask0Stack[217];


unsigned int gValue=0x30;

/**
 * task0 function
 */
__attribute__((aligned(4))) void task0Func(void){
  volatile unsigned int regSP;
  printf_("enter task0\n");
  __asm__ __volatile__("mv %0, sp":"=r"(regSP));
  printf_("task0 sp: 0x%x\n", regSP);
  while(1) {
    GPIO_ODR ^= 0x04; 
    mydelay();
    yield();
  }  
}


/**
 * task1 function
 */
__attribute__((aligned(4))) void task1Func(void){
  volatile unsigned int regSP;
  printf_("enter task1\n");
  __asm__ __volatile__("mv %0, sp":"=r"(regSP));
  printf_("task1 sp: 0x%x\n", regSP);

  while(1) {
    printf_("task1: %d\n", gValue);
    gValue ++;

    mydelay();
    yield();
  }  
}


/**
 * function for task stack initial ra target
 */
__attribute__((aligned(4))) void halSysExit(void){
  UART0_DR = 0x61;
  GPIO_ODR = 0x00;    
  while(1) {
  }  
}

/**
 * invoke ecall
 */
void invokeEcall(unsigned int argA0, unsigned int argA1){
  __asm__ __volatile__("ecall");
}

/**
 * switch task
 */
void yield(){
  invokeEcall(3, 0);
}

/**
 * main function
 */
void main() {
  
  GPIO_ODR = 0x00;   
  //
  //init uart0
  //baudrate=115200, 115200=fclk/(16*n)
  //fclk = 80M or 160M
  //n1=80M/16/115200=43.40278; 0.40278*64+0.5=26
  //n2=160M/16/115200=86.80556; 0.80556*64+0.5=52
  UART0_CR = 0;
  UART0_LCR_H = 0x0060; //bit[6:5]=11
  UART0_IBRD = 43;
  UART0_FBRD = 26;
  UART0_CR = 0x0301; //txen, rxen, uarten
  
  printf_("start...\n");
  
  //init task related structure
  initTCB();
  
  TaskContext *context;
  
  gTCB0.sp -= 32*4;
  context = (TaskContext *)(gTCB0.sp);
  initTaskContext(context);
  context -> mepc = gTCB0.mepc;

  gTCB1.sp -= 32*4;
  context = (TaskContext *)(gTCB1.sp);
  initTaskContext(context);
  context -> mepc = gTCB1.mepc;
  
  //switch task stack
  unsigned int i;
  i = gTCB0.sp;
  __asm__ __volatile__("mv sp, %0":"=r"(i));
  //run task0 function
  task0Func();


  
  //failed
  while(1) {
    GPIO_ODR = 0x00;   
    mydelay();
  }
}

void mydelay(void) {
  unsigned int i;
  for (i=0;i<10000000;i++) {
    __asm__ __volatile__("nop");
  }
}

void _putchar(char character) {
  //wait for tx fifo not full
  while (UART0_FR&(0x1<<5)) { //tx fifo full flag
    __asm__ __volatile__("nop");
  }
  UART0_DR = character;

}

/**
 * actual isr
 * handle exception and interrupt other than ecall
 */
void myisr(void) {
  volatile unsigned int mcause;
  volatile unsigned int mepc;
  volatile unsigned int mtval;

  __asm__ __volatile__("csrr %0, mcause":"=r"(mcause));
  
  if (mcause == 11) { //m ecall exception, should not reach here
    __asm__ __volatile__("csrr %0, mepc":"=r"(mepc));
    mepc = mepc + 4;
    __asm__ __volatile__("csrw mepc, %0":"=r"(mepc));
  }else {
    printf_("unexpected mcause: 0x%x.\n", mcause);
    __asm__ __volatile__("csrr %0, mtval":"=r"(mtval));
    printf_("mtval: 0x%x, halt!\n", mtval);
    while(1) {
    }  
  }
}


/**
 * find and set gTaskCtrl.nextTask
 */
TaskCtrl* switchCurrentTask(void){
//  printf_("enter switchCurrentTask\n");
  if (gCurrentTaskId == 0) {
    gTaskCtrl.nextTask = &gTCB1;
    gCurrentTaskId = 1;
  }else {
    gTaskCtrl.nextTask = &gTCB0;
    gCurrentTaskId = 0;
  }  
//   printf_("return: 0x%x\n", &gTaskCtrl);
 
  return &gTaskCtrl;
}

/**
 * init task0 and task1's tcb
 */
void initTCB(void) {
  gCurrentTaskId = 0;
  gTCB0.sp = (unsigned int)(unsigned int *)&(gTask0Stack[200]);
  gTCB0.mepc = (unsigned int)(unsigned int *)task0Func;
  gTCB1.sp = (unsigned int)(unsigned int *)&(gTask1Stack[200]);
  gTCB1.mepc = (unsigned int)(unsigned int *)task1Func;
  gTaskCtrl.currentTask = &gTCB0;
  gTaskCtrl.nextTask = &gTCB1;
  
}

/**
 * init task stack context to default value
 */
void initTaskContext(TaskContext *context) {
  context->mstatus = (1<<11)|(1<<7);
//  context->mepc = 0;
  context->tp = TP_INIT_VALUE;
  context->sp = SP_INIT_VALUE;
  context->s11 = S11_INIT_VALUE;
  context->s10 = S10_INIT_VALUE;
  context->s9 = S9_INIT_VALUE;
  context->s8 = S8_INIT_VALUE;
  context->s7 = S7_INIT_VALUE;
  context->s6 = S6_INIT_VALUE;
  context->s5 = S5_INIT_VALUE;
  context->s4 = S4_INIT_VALUE;
  context->s3 = S3_INIT_VALUE;
  context->s2 = S2_INIT_VALUE;
  context->s1 = S1_INIT_VALUE;
  context->s0 = FP_INIT_VALUE;
  context->t6 = T6_INIT_VALUE;
  context->t5 = T5_INIT_VALUE;
  context->t4 = T4_INIT_VALUE;
  context->t3 = T3_INIT_VALUE;
  context->a7 = A7_INIT_VALUE;
  context->a6 = A6_INIT_VALUE;
  context->a5 = A5_INIT_VALUE;
  context->a4 = A4_INIT_VALUE;
  context->a3 = A3_INIT_VALUE;
  context->a2 = A2_INIT_VALUE;
  context->a1 = A1_INIT_VALUE;
  context->a0 = 0;
  context->t2 = T2_INIT_VALUE;
  context->t1 = T1_INIT_VALUE;
  context->t0 = T0_INIT_VALUE;
  context->ra = (unsigned int)(unsigned int *)halSysExit;
}

/**
 * for .s debug
 */
void showMsg(unsigned int i, unsigned int data) {
  if (i==0) {
    printf_("enter isrWrapper\n");
  }else if (i==1) {
    printf_("mcause:0x%x\n", data);
  }else {
    printf_("i=%d: data=0x%x\n", i, data);
  }
}